using System;
using System.Drawing;
using System.Windows.Forms;
using System.Threading;
using FIRADriverLibrary;
using Geometry;
using System.Collections.Generic;
using PotentialFieldDriver.robotrace.drivers.implementation;
using RoBOSSCommunicator;
using PotentialFieldDriver.robotrace.environment.implementation;
using PotentialFieldDriver.robotrace.utils.pitch;
using PotentialFieldDriver.robotrace.utils.fields;

namespace PotentialFieldDriver
{
    public class PotencialDriverForm : System.Windows.Forms.Form
    {
        private const float robotDiameter = 0.075f;
        private const float ballDiameter = 0.036f;
        private System.ComponentModel.IContainer components;

        private System.Windows.Forms.Panel legtPanel;
        private System.Windows.Forms.Panel viewPanel;
        private System.Windows.Forms.Splitter splitter1;
        private System.Windows.Forms.ListBox robotsListBox;
        private System.Windows.Forms.Label label1;
        private System.Windows.Forms.Label statuSLabel;
        private System.Windows.Forms.Button requestButton;
        private System.Windows.Forms.Label rightStatusLabel;
        private System.Windows.Forms.Label middleStatusLabel;
        private System.Windows.Forms.Button releaseButton;
        private System.Windows.Forms.Timer interfaceRefreshTimer;

        private double oldTime;
        private Thread refreshThread = null;
        private bool killThread = false;
        private Mutex firaDriverMutex = null;

        private FIRADriver firaDriver;
        private RectangleF pitchArea;
        private float goalWigth;

        private volatile Bitmap viewBitmap;
        private Graphics viewPanelGraphics;

        private float viewScale;
        private PointF viewOffset;

        private Brush pithBrush = (new Pen(Color.Black)).Brush;
        private Pen pithPen = new Pen(Color.LightGray, 3);
        private Pen robotPen = new Pen(Color.LightGreen, 1);
        private Pen theRobotPen = new Pen(Color.Blue, 2);
        private Pen ballPen = new Pen(Color.Orange, 1);
        private Pen targetPen = new Pen(Color.Red, 1);

        private Pen wallPen = new Pen(Color.Salmon, 3);
        private Brush additionalPointsBrush = (new Pen(Color.Salmon, 0.0f)).Brush;

        private int robotDiameterView;
        private int ballDiameterView;

        //This should be in another classes (Class controlling robots should be extracted)
        private List<Point2D> targets = new List<Point2D>();
        private List<ControlledRobot> controlledRobots = new List<ControlledRobot>();

        private System.Windows.Forms.Button moveRobotButton;
        private System.Windows.Forms.Label targetLocationLabel;
        private System.Windows.Forms.Label label2;
        private System.Windows.Forms.Label label3;
        private System.Windows.Forms.NumericUpDown fpsNumericUpDown;

        private System.Windows.Forms.GroupBox pointObstaclesGroupBox;
        private System.Windows.Forms.GroupBox groupBox1;
        private System.Windows.Forms.Button clearPointsButton;
        private System.Windows.Forms.Button clearWallsButton;
        private System.Windows.Forms.Button pitchBoundsButton;
        private System.Windows.Forms.CheckBox addPointCheckBox;
        private System.Windows.Forms.CheckBox addWallCheckBox;
        private System.Windows.Forms.NumericUpDown velocityNumericUpDown;
        Point2D targetPoint;



        private Pen targetPathPen = new Pen(Color.Yellow, 1);
        private Pen currentTargetPathPen = new Pen(Color.White, 1);


        private List<Point2D> visited = new List<Point2D>();
        private Button restPathButton;
        private Button optimizePathButton;
        private Button pitchBoundsButton2;
        private Pen visitedPen = new Pen(Color.YellowGreen, 1);
        private Button ShowBitmap;
        private Pitch pitch;
        private bool showBitmap = false;
        private bool showPotentialField = false;
        private Brush obstaclesBrush = (new Pen(Color.Red, 0.0f)).Brush;
        private Brush nonReachableBrush = (new Pen(Color.Goldenrod, 0.0f)).Brush;
        private int noOfColumnsInGrid = 200;
        private int noOfRowsInGrid = 100;
        private Button showPotentialFieldButton;
        private NumericUpDown wallRadix;
        private List<PotentialField> potentialFields;
        private Button stopRobotButton;
        private RobotDriverInterface driver;

        [STAThread]
        static void Main()
        {
            CreateFiraDriverForm createFiraDriverForm = new CreateFiraDriverForm();
            createFiraDriverForm.ShowDialog();
            FIRADriver firaDriver = createFiraDriverForm.FiraDriver;
            Communicator communicator = createFiraDriverForm.Communicator;
            if (firaDriver != null)
            {
                Application.Run(new PotencialDriverForm(firaDriver, communicator, createFiraDriverForm.PithArea, createFiraDriverForm.GoalWidth));
                firaDriver.Dispose();
            }
        }

        public PotencialDriverForm(FIRADriver firaDriver, Communicator communicator, RectangleF pitchArea, float goalWigth)
        {
            this.firaDriver = firaDriver;
            this.goalWigth = goalWigth;

            this.pitch = new Pitch(communicator);
            this.pitch.getObstaclesBitmap(noOfColumnsInGrid, noOfRowsInGrid);
            this.pitchArea = pitch.getPitchArea();
            InitializeComponent();



            FillRobotList();

            CreateViewDrawer();

            PotencialDriver.firaDriver = firaDriver;
            PotencialDriver.pitchArea = pitchArea;


            firaDriverMutex = new Mutex();
            refreshThread = new Thread(new ThreadStart(this.RefreshThread));
            refreshThread.Priority = ThreadPriority.AboveNormal;
            refreshThread.Start();

            interfaceRefreshTimer.Enabled = true;
        }

        private void RefreshThread()
        {
            driver = new PotentialFieldBasedDriver(pitch, targets, Convert.ToInt32(wallRadix.Value));
            //driver = new SimpleGoToTheNextPointDriver(targets);
            oldTime = -1.0;
            while (true)
            {

                firaDriverMutex.WaitOne();
                firaDriver.Step();
                if (true)//firaDriver.time > oldTime)
                {

                    foreach (ControlledRobot robot in controlledRobots)

                    //if (movingRobotId >= 0)
                    {
                        if (robot.getIsMoving())
                        {
                            if (targets.Count <= 0)
                            {
                                robot.setCurrentTarget(0);
                                //currentTarget = 0;
                                targets.Add(targetPoint);
                                break;
                            }
                            else
                            {
                                if (robot.getCurrentTarget() >= targets.Count)
                                //if (currentTarget >= targets.Count)
                                {
                                    robot.setCurrentTarget(0);
                                    //currentTarget = 0;
                                }
                                //add current position to visited
                                //visited.Add(new Point2D(firaDriver.firaRobots[movingRobotId].x, firaDriver.firaRobots[movingRobotId].y));
                                visited.Add(new Point2D(firaDriver.firaRobots[robot.getId()].x, firaDriver.firaRobots[robot.getId()].y));
                                while (visited.Count > 20)
                                {
                                    visited.RemoveAt(0);
                                }

                                //if target close to current location, switch to next one
                                if (targets[robot.getCurrentTarget()].GetDistance(firaDriver.firaRobots[robot.getId()].x, firaDriver.firaRobots[robot.getId()].y) < 0.20)
                                //if (targets[currentTarget].GetDistance(firaDriver.firaRobots[movingRobotId].x, firaDriver.firaRobots[movingRobotId].y) < 0.20)
                                {
                                    //
                                    robot.setCurrentTarget(robot.getCurrentTarget() + 1);
                                    //currentTarget++;
                                    if (robot.getCurrentTarget() >= targets.Count)
                                    //if (currentTarget >= targets.Count)
                                    {
                                        robot.setCurrentTarget(0);
                                        //currentTarget = 0;
                                    }
                                }

                                //TU JEST PRZESUWANIE ROBOTA. WLASCIWIE CALA TA FUNKCJA WEJDZIE DO DRIVERA, (zrobie commita jak zacznie mi to jakos dzialac...)
                                //move robot
                                //PotencialDriver.DriveVelocity(firaDriver.firaRobots[movingRobotId], targets[currentTarget].x, targets[currentTarget].y, (double)velocityNumericUpDown.Value);
                                driver.step(firaDriver.firaRobots[robot.getId()]);
                                //driver.step(firaDriver.firaRobots[movingRobotId]);
                            }
                        }
                    }

                    //rightStatusLabel.Text = firaDriver.time.ToString("f3");
                    oldTime = firaDriver.time + 1 / (double)fpsNumericUpDown.Value;
                    firaDriverMutex.ReleaseMutex();	//after
                }
                else
                {
                    firaDriverMutex.ReleaseMutex();	//before
                    Thread.Sleep(1);
                }
                if (killThread)
                    return;
            }
        }


        private void FillRobotList()
        {
            foreach (FIRARobotState robot in firaDriver.firaRobots)
            {
                robotsListBox.Items.Add(robot);
            }
        }

        private void CreateViewDrawer()
        {
            if (viewBitmap != null)
                viewBitmap.Dispose();
            if (viewPanelGraphics != null)
                viewPanelGraphics.Dispose();

            viewPanelGraphics = viewPanel.CreateGraphics();
            viewPanelGraphics.Clear(SystemColors.ControlDarkDark);
            viewBitmap = new Bitmap(Math.Max(1, viewPanel.Width), Math.Max(1, viewPanel.Height));
            Graphics viewBitmapGraphics = Graphics.FromImage(viewBitmap);

            viewScale = (float)Math.Min(viewPanel.Width / (1.2f * pitchArea.Width),
                viewPanel.Height / (1.1f * pitchArea.Height));
            viewOffset = new PointF(-pitchArea.X + pitchArea.Width * 0.1f,
                -pitchArea.Y + pitchArea.Width * 0.05f);


            //goals border
            viewBitmapGraphics.DrawRectangle(pithPen, Real2ViewX(pitchArea.X - pitchArea.Width * 0.05f), Real2ViewY(pitchArea.Y + pitchArea.Height / 2 - goalWigth / 2),
                (pitchArea.Width * 1.1f) * viewScale, goalWigth * viewScale);
            //pitch
            viewBitmapGraphics.FillRectangle(pithBrush, Real2ViewX(pitchArea.X), Real2ViewY(pitchArea.Y),
                pitchArea.Width * viewScale, pitchArea.Height * viewScale);
            viewBitmapGraphics.DrawRectangle(pithPen, Real2ViewX(pitchArea.X), Real2ViewY(pitchArea.Y),
                pitchArea.Width * viewScale, pitchArea.Height * viewScale);

            //goals
            viewBitmapGraphics.FillRectangle(pithBrush, Real2ViewX(pitchArea.X - pitchArea.Width * 0.05f), Real2ViewY(pitchArea.Y + pitchArea.Height / 2 - goalWigth / 2),
                (pitchArea.Width * 1.1f) * viewScale, goalWigth * viewScale);

            robotDiameterView = (int)(robotDiameter * viewScale);
            ballDiameterView = (int)(ballDiameter * viewScale);


            viewBitmapGraphics.Dispose();
            viewPanel_Paint(this, null);
        }

        private int Real2ViewX(double realX)
        {
            return (int)((realX + viewOffset.X) * viewScale);
        }

        private int Real2ViewY(double realY)
        {
            return (int)((realY + viewOffset.Y) * viewScale);
        }


        private double View2RealX(int x)
        {
            return (double)x / viewScale - viewOffset.X;
        }
        private double View2RealY(int y)
        {
            return (double)y / viewScale - viewOffset.Y;
        }




        protected override void Dispose(bool disposing)
        {
            interfaceRefreshTimer.Enabled = false;
            killThread = true;

            if (disposing)
            {
                if (components != null)
                {
                    components.Dispose();
                }
            }
            base.Dispose(disposing);
        }

        #region Windows Form Designer generated code
        /// <summary>
        /// Required method for Designer support - do not modify
        /// the contents of this method with the code editor.
        /// </summary>
        private void InitializeComponent()
        {
            this.components = new System.ComponentModel.Container();
            this.legtPanel = new System.Windows.Forms.Panel();
            this.wallRadix = new System.Windows.Forms.NumericUpDown();
            this.showPotentialFieldButton = new System.Windows.Forms.Button();
            this.ShowBitmap = new System.Windows.Forms.Button();
            this.optimizePathButton = new System.Windows.Forms.Button();
            this.restPathButton = new System.Windows.Forms.Button();
            this.pointObstaclesGroupBox = new System.Windows.Forms.GroupBox();
            this.addPointCheckBox = new System.Windows.Forms.CheckBox();
            this.clearPointsButton = new System.Windows.Forms.Button();
            this.fpsNumericUpDown = new System.Windows.Forms.NumericUpDown();
            this.label3 = new System.Windows.Forms.Label();
            this.velocityNumericUpDown = new System.Windows.Forms.NumericUpDown();
            this.label2 = new System.Windows.Forms.Label();
            this.targetLocationLabel = new System.Windows.Forms.Label();
            this.moveRobotButton = new System.Windows.Forms.Button();
            this.requestButton = new System.Windows.Forms.Button();
            this.label1 = new System.Windows.Forms.Label();
            this.robotsListBox = new System.Windows.Forms.ListBox();
            this.releaseButton = new System.Windows.Forms.Button();
            this.groupBox1 = new System.Windows.Forms.GroupBox();
            this.pitchBoundsButton2 = new System.Windows.Forms.Button();
            this.clearWallsButton = new System.Windows.Forms.Button();
            this.pitchBoundsButton = new System.Windows.Forms.Button();
            this.addWallCheckBox = new System.Windows.Forms.CheckBox();
            this.viewPanel = new System.Windows.Forms.Panel();
            this.splitter1 = new System.Windows.Forms.Splitter();
            this.statuSLabel = new System.Windows.Forms.Label();
            this.rightStatusLabel = new System.Windows.Forms.Label();
            this.middleStatusLabel = new System.Windows.Forms.Label();
            this.interfaceRefreshTimer = new System.Windows.Forms.Timer(this.components);
            this.stopRobotButton = new System.Windows.Forms.Button();
            this.legtPanel.SuspendLayout();
            ((System.ComponentModel.ISupportInitialize)(this.wallRadix)).BeginInit();
            this.pointObstaclesGroupBox.SuspendLayout();
            ((System.ComponentModel.ISupportInitialize)(this.fpsNumericUpDown)).BeginInit();
            ((System.ComponentModel.ISupportInitialize)(this.velocityNumericUpDown)).BeginInit();
            this.groupBox1.SuspendLayout();
            this.SuspendLayout();
            // 
            // legtPanel
            // 
            this.legtPanel.Controls.Add(this.stopRobotButton);
            this.legtPanel.Controls.Add(this.wallRadix);
            this.legtPanel.Controls.Add(this.showPotentialFieldButton);
            this.legtPanel.Controls.Add(this.ShowBitmap);
            this.legtPanel.Controls.Add(this.optimizePathButton);
            this.legtPanel.Controls.Add(this.restPathButton);
            this.legtPanel.Controls.Add(this.pointObstaclesGroupBox);
            this.legtPanel.Controls.Add(this.fpsNumericUpDown);
            this.legtPanel.Controls.Add(this.label3);
            this.legtPanel.Controls.Add(this.velocityNumericUpDown);
            this.legtPanel.Controls.Add(this.label2);
            this.legtPanel.Controls.Add(this.targetLocationLabel);
            this.legtPanel.Controls.Add(this.moveRobotButton);
            this.legtPanel.Controls.Add(this.requestButton);
            this.legtPanel.Controls.Add(this.label1);
            this.legtPanel.Controls.Add(this.robotsListBox);
            this.legtPanel.Controls.Add(this.releaseButton);
            this.legtPanel.Controls.Add(this.groupBox1);
            this.legtPanel.Dock = System.Windows.Forms.DockStyle.Left;
            this.legtPanel.Location = new System.Drawing.Point(0, 0);
            this.legtPanel.Name = "legtPanel";
            this.legtPanel.Size = new System.Drawing.Size(184, 457);
            this.legtPanel.TabIndex = 0;
            // 
            // wallRadix
            // 
            this.wallRadix.DecimalPlaces = 1;
            this.wallRadix.Location = new System.Drawing.Point(94, 172);
            this.wallRadix.Name = "wallRadix";
            this.wallRadix.Size = new System.Drawing.Size(48, 20);
            this.wallRadix.TabIndex = 14;
            this.wallRadix.Value = new decimal(new int[] {
            5,
            0,
            0,
            0});
            this.wallRadix.ValueChanged += new System.EventHandler(this.numericUpDown1_ValueChanged);
            // 
            // showPotentialFieldButton
            // 
            this.showPotentialFieldButton.Location = new System.Drawing.Point(91, 201);
            this.showPotentialFieldButton.Name = "showPotentialFieldButton";
            this.showPotentialFieldButton.Size = new System.Drawing.Size(85, 23);
            this.showPotentialFieldButton.TabIndex = 13;
            this.showPotentialFieldButton.Text = "Show PF";
            this.showPotentialFieldButton.UseVisualStyleBackColor = true;
            this.showPotentialFieldButton.Click += new System.EventHandler(this.showPotentialField_Click);
            // 
            // ShowBitmap
            // 
            this.ShowBitmap.Location = new System.Drawing.Point(3, 201);
            this.ShowBitmap.Name = "ShowBitmap";
            this.ShowBitmap.Size = new System.Drawing.Size(85, 23);
            this.ShowBitmap.TabIndex = 12;
            this.ShowBitmap.Text = "Show Bitmap";
            this.ShowBitmap.UseVisualStyleBackColor = true;
            this.ShowBitmap.Click += new System.EventHandler(this.ShowBitmap_Click);
            // 
            // optimizePathButton
            // 
            this.optimizePathButton.Location = new System.Drawing.Point(3, 172);
            this.optimizePathButton.Name = "optimizePathButton";
            this.optimizePathButton.Size = new System.Drawing.Size(85, 23);
            this.optimizePathButton.TabIndex = 11;
            this.optimizePathButton.Text = "Optimize Path";
            this.optimizePathButton.UseVisualStyleBackColor = true;
            this.optimizePathButton.Click += new System.EventHandler(this.optimizePathButton_Click);
            // 
            // restPathButton
            // 
            this.restPathButton.Location = new System.Drawing.Point(3, 143);
            this.restPathButton.Name = "restPathButton";
            this.restPathButton.Size = new System.Drawing.Size(85, 23);
            this.restPathButton.TabIndex = 10;
            this.restPathButton.Text = "ResetPath";
            this.restPathButton.UseVisualStyleBackColor = true;
            this.restPathButton.Click += new System.EventHandler(this.restPathButton_Click);
            // 
            // pointObstaclesGroupBox
            // 
            this.pointObstaclesGroupBox.Controls.Add(this.addPointCheckBox);
            this.pointObstaclesGroupBox.Controls.Add(this.clearPointsButton);
            this.pointObstaclesGroupBox.Location = new System.Drawing.Point(0, 297);
            this.pointObstaclesGroupBox.Name = "pointObstaclesGroupBox";
            this.pointObstaclesGroupBox.Size = new System.Drawing.Size(184, 54);
            this.pointObstaclesGroupBox.TabIndex = 9;
            this.pointObstaclesGroupBox.TabStop = false;
            this.pointObstaclesGroupBox.Text = "point obstacles";
            // 
            // addPointCheckBox
            // 
            this.addPointCheckBox.Appearance = System.Windows.Forms.Appearance.Button;
            this.addPointCheckBox.Location = new System.Drawing.Point(144, 16);
            this.addPointCheckBox.Name = "addPointCheckBox";
            this.addPointCheckBox.Size = new System.Drawing.Size(32, 24);
            this.addPointCheckBox.TabIndex = 1;
            this.addPointCheckBox.Text = "add";
            // 
            // clearPointsButton
            // 
            this.clearPointsButton.Location = new System.Drawing.Point(96, 16);
            this.clearPointsButton.Name = "clearPointsButton";
            this.clearPointsButton.Size = new System.Drawing.Size(40, 23);
            this.clearPointsButton.TabIndex = 0;
            this.clearPointsButton.Text = "clear";
            this.clearPointsButton.Click += new System.EventHandler(this.clearPointsButton_Click);
            // 
            // fpsNumericUpDown
            // 
            this.fpsNumericUpDown.Location = new System.Drawing.Point(112, 8);
            this.fpsNumericUpDown.Maximum = new decimal(new int[] {
            500,
            0,
            0,
            0});
            this.fpsNumericUpDown.Minimum = new decimal(new int[] {
            1,
            0,
            0,
            0});
            this.fpsNumericUpDown.Name = "fpsNumericUpDown";
            this.fpsNumericUpDown.Size = new System.Drawing.Size(64, 20);
            this.fpsNumericUpDown.TabIndex = 8;
            this.fpsNumericUpDown.Value = new decimal(new int[] {
            25,
            0,
            0,
            0});
            // 
            // label3
            // 
            this.label3.Location = new System.Drawing.Point(8, 8);
            this.label3.Name = "label3";
            this.label3.Size = new System.Drawing.Size(104, 23);
            this.label3.TabIndex = 7;
            this.label3.Text = "communicator FPS:";
            this.label3.TextAlign = System.Drawing.ContentAlignment.MiddleLeft;
            // 
            // velocityNumericUpDown
            // 
            this.velocityNumericUpDown.DecimalPlaces = 1;
            this.velocityNumericUpDown.Increment = new decimal(new int[] {
            1,
            0,
            0,
            65536});
            this.velocityNumericUpDown.Location = new System.Drawing.Point(40, 269);
            this.velocityNumericUpDown.Maximum = new decimal(new int[] {
            2,
            0,
            0,
            0});
            this.velocityNumericUpDown.Minimum = new decimal(new int[] {
            1,
            0,
            0,
            65536});
            this.velocityNumericUpDown.Name = "velocityNumericUpDown";
            this.velocityNumericUpDown.Size = new System.Drawing.Size(48, 20);
            this.velocityNumericUpDown.TabIndex = 6;
            this.velocityNumericUpDown.Value = new decimal(new int[] {
            5,
            0,
            0,
            65536});
            // 
            // label2
            // 
            this.label2.Location = new System.Drawing.Point(8, 271);
            this.label2.Name = "label2";
            this.label2.Size = new System.Drawing.Size(36, 23);
            this.label2.TabIndex = 5;
            this.label2.Text = "vel:";
            // 
            // targetLocationLabel
            // 
            this.targetLocationLabel.Location = new System.Drawing.Point(8, 231);
            this.targetLocationLabel.Name = "targetLocationLabel";
            this.targetLocationLabel.Size = new System.Drawing.Size(168, 40);
            this.targetLocationLabel.TabIndex = 4;
            this.targetLocationLabel.Text = "added point:";
            // 
            // moveRobotButton
            // 
            this.moveRobotButton.Location = new System.Drawing.Point(91, 272);
            this.moveRobotButton.Name = "moveRobotButton";
            this.moveRobotButton.Size = new System.Drawing.Size(45, 22);
            this.moveRobotButton.TabIndex = 3;
            this.moveRobotButton.Text = "move";
            this.moveRobotButton.Click += new System.EventHandler(this.moveRobotButton_Click);
            // 
            // requestButton
            // 
            this.requestButton.Location = new System.Drawing.Point(3, 114);
            this.requestButton.Name = "requestButton";
            this.requestButton.Size = new System.Drawing.Size(56, 23);
            this.requestButton.TabIndex = 2;
            this.requestButton.Text = "request";
            this.requestButton.Click += new System.EventHandler(this.requestButton_Click);
            // 
            // label1
            // 
            this.label1.Location = new System.Drawing.Point(0, 40);
            this.label1.Name = "label1";
            this.label1.Size = new System.Drawing.Size(48, 16);
            this.label1.TabIndex = 1;
            this.label1.Text = "robots:";
            // 
            // robotsListBox
            // 
            this.robotsListBox.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
                        | System.Windows.Forms.AnchorStyles.Right)));
            this.robotsListBox.Font = new System.Drawing.Font("Microsoft Sans Serif", 9.75F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(238)));
            this.robotsListBox.ItemHeight = 16;
            this.robotsListBox.Location = new System.Drawing.Point(0, 56);
            this.robotsListBox.Name = "robotsListBox";
            this.robotsListBox.Size = new System.Drawing.Size(184, 52);
            this.robotsListBox.TabIndex = 0;
            // 
            // releaseButton
            // 
            this.releaseButton.Location = new System.Drawing.Point(65, 114);
            this.releaseButton.Name = "releaseButton";
            this.releaseButton.Size = new System.Drawing.Size(56, 23);
            this.releaseButton.TabIndex = 2;
            this.releaseButton.Text = "release";
            this.releaseButton.Click += new System.EventHandler(this.releaseButton_Click);
            // 
            // groupBox1
            // 
            this.groupBox1.Controls.Add(this.pitchBoundsButton2);
            this.groupBox1.Controls.Add(this.clearWallsButton);
            this.groupBox1.Controls.Add(this.pitchBoundsButton);
            this.groupBox1.Controls.Add(this.addWallCheckBox);
            this.groupBox1.Location = new System.Drawing.Point(0, 357);
            this.groupBox1.Name = "groupBox1";
            this.groupBox1.Size = new System.Drawing.Size(184, 99);
            this.groupBox1.TabIndex = 9;
            this.groupBox1.TabStop = false;
            this.groupBox1.Text = "line obstacles";
            // 
            // pitchBoundsButton2
            // 
            this.pitchBoundsButton2.Location = new System.Drawing.Point(8, 70);
            this.pitchBoundsButton2.Name = "pitchBoundsButton2";
            this.pitchBoundsButton2.Size = new System.Drawing.Size(80, 23);
            this.pitchBoundsButton2.TabIndex = 2;
            this.pitchBoundsButton2.Text = "Track 2";
            this.pitchBoundsButton2.Click += new System.EventHandler(this.pitchBoundsButton2_Click);
            // 
            // clearWallsButton
            // 
            this.clearWallsButton.Location = new System.Drawing.Point(96, 16);
            this.clearWallsButton.Name = "clearWallsButton";
            this.clearWallsButton.Size = new System.Drawing.Size(40, 23);
            this.clearWallsButton.TabIndex = 0;
            this.clearWallsButton.Text = "clear";
            this.clearWallsButton.Click += new System.EventHandler(this.clearWallsButton_Click);
            // 
            // pitchBoundsButton
            // 
            this.pitchBoundsButton.Location = new System.Drawing.Point(8, 41);
            this.pitchBoundsButton.Name = "pitchBoundsButton";
            this.pitchBoundsButton.Size = new System.Drawing.Size(80, 23);
            this.pitchBoundsButton.TabIndex = 0;
            this.pitchBoundsButton.Text = "Track 1";
            this.pitchBoundsButton.Click += new System.EventHandler(this.pitchBoundsButton_Click);
            // 
            // addWallCheckBox
            // 
            this.addWallCheckBox.Appearance = System.Windows.Forms.Appearance.Button;
            this.addWallCheckBox.Location = new System.Drawing.Point(144, 16);
            this.addWallCheckBox.Name = "addWallCheckBox";
            this.addWallCheckBox.Size = new System.Drawing.Size(32, 24);
            this.addWallCheckBox.TabIndex = 1;
            this.addWallCheckBox.Text = "add";
            // 
            // viewPanel
            // 
            this.viewPanel.BackColor = System.Drawing.SystemColors.ControlDarkDark;
            this.viewPanel.Dock = System.Windows.Forms.DockStyle.Fill;
            this.viewPanel.Location = new System.Drawing.Point(187, 0);
            this.viewPanel.Name = "viewPanel";
            this.viewPanel.Size = new System.Drawing.Size(581, 457);
            this.viewPanel.TabIndex = 1;
            this.viewPanel.Paint += new System.Windows.Forms.PaintEventHandler(this.viewPanel_Paint);
            this.viewPanel.MouseUp += new System.Windows.Forms.MouseEventHandler(this.viewPanel_MouseUp);
            this.viewPanel.Resize += new System.EventHandler(this.viewPanel_Resize);
            // 
            // splitter1
            // 
            this.splitter1.Location = new System.Drawing.Point(184, 0);
            this.splitter1.MinExtra = 184;
            this.splitter1.MinSize = 184;
            this.splitter1.Name = "splitter1";
            this.splitter1.Size = new System.Drawing.Size(3, 457);
            this.splitter1.TabIndex = 2;
            this.splitter1.TabStop = false;
            // 
            // statuSLabel
            // 
            this.statuSLabel.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)
                        | System.Windows.Forms.AnchorStyles.Right)));
            this.statuSLabel.ForeColor = System.Drawing.SystemColors.ControlDarkDark;
            this.statuSLabel.Location = new System.Drawing.Point(0, 457);
            this.statuSLabel.Name = "statuSLabel";
            this.statuSLabel.Size = new System.Drawing.Size(328, 12);
            this.statuSLabel.TabIndex = 3;
            // 
            // rightStatusLabel
            // 
            this.rightStatusLabel.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Right)));
            this.rightStatusLabel.ForeColor = System.Drawing.SystemColors.ControlDarkDark;
            this.rightStatusLabel.Location = new System.Drawing.Point(656, 457);
            this.rightStatusLabel.Name = "rightStatusLabel";
            this.rightStatusLabel.Size = new System.Drawing.Size(112, 12);
            this.rightStatusLabel.TabIndex = 4;
            this.rightStatusLabel.TextAlign = System.Drawing.ContentAlignment.TopRight;
            this.rightStatusLabel.DoubleClick += new System.EventHandler(this.rightStatusLabel_DoubleClick);
            // 
            // middleStatusLabel
            // 
            this.middleStatusLabel.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Right)));
            this.middleStatusLabel.ForeColor = System.Drawing.SystemColors.ControlDarkDark;
            this.middleStatusLabel.Location = new System.Drawing.Point(328, 457);
            this.middleStatusLabel.Name = "middleStatusLabel";
            this.middleStatusLabel.Size = new System.Drawing.Size(200, 12);
            this.middleStatusLabel.TabIndex = 5;
            // 
            // interfaceRefreshTimer
            // 
            this.interfaceRefreshTimer.Interval = 200;
            this.interfaceRefreshTimer.Tick += new System.EventHandler(this.interfaceRefreshTimer_Tick);
            // 
            // stopRobotButton
            // 
            this.stopRobotButton.Location = new System.Drawing.Point(139, 272);
            this.stopRobotButton.Name = "stopRobotButton";
            this.stopRobotButton.Size = new System.Drawing.Size(39, 22);
            this.stopRobotButton.TabIndex = 15;
            this.stopRobotButton.Text = "stop";
            this.stopRobotButton.Click += new System.EventHandler(this.stopRobotButton_Click);
            // 
            // PotencialDriverForm
            // 
            this.AutoScaleBaseSize = new System.Drawing.Size(5, 13);
            this.ClientSize = new System.Drawing.Size(768, 469);
            this.Controls.Add(this.middleStatusLabel);
            this.Controls.Add(this.statuSLabel);
            this.Controls.Add(this.rightStatusLabel);
            this.Controls.Add(this.viewPanel);
            this.Controls.Add(this.splitter1);
            this.Controls.Add(this.legtPanel);
            this.MinimumSize = new System.Drawing.Size(592, 352);
            this.Name = "PotencialDriverForm";
            this.Padding = new System.Windows.Forms.Padding(0, 0, 0, 12);
            this.Text = "Potencial Driver";
            this.Load += new System.EventHandler(this.PotencialDriverForm_Load);
            this.legtPanel.ResumeLayout(false);
            ((System.ComponentModel.ISupportInitialize)(this.wallRadix)).EndInit();
            this.pointObstaclesGroupBox.ResumeLayout(false);
            ((System.ComponentModel.ISupportInitialize)(this.fpsNumericUpDown)).EndInit();
            ((System.ComponentModel.ISupportInitialize)(this.velocityNumericUpDown)).EndInit();
            this.groupBox1.ResumeLayout(false);
            this.ResumeLayout(false);

        }
        #endregion

        private void requestButton_Click(object sender, System.EventArgs e)
        {
            FIRARobotState robot = robotsListBox.SelectedItem as FIRARobotState;
            if (robot == null)
                return;
            if (robot.owned)
            {
                MessageBox.Show("Robot already granted.");
                return;
            }
            this.Cursor = Cursors.WaitCursor;
            firaDriverMutex.WaitOne();
            if (firaDriver.RequestRobot(robot.portId) < 0)
            {
                MessageBox.Show("Error requesting robot: " + FIRADriver.lastError);
            }
            controlledRobots.Add(new ControlledRobot(robot.portId));
            firaDriverMutex.ReleaseMutex();
            this.Cursor = Cursors.Default;

            statuSLabel.Text = "Robot " + robot.portId + " granted!.";
        }

        private void releaseButton_Click(object sender, System.EventArgs e)
        {
            FIRARobotState robot = robotsListBox.SelectedItem as FIRARobotState;
            if (robot == null)
                return;
            if (!robot.owned)
            {
                MessageBox.Show("Robot not owned.");
                return;
            }
            firaDriverMutex.WaitOne();
            firaDriver.ReleaseRobot(robot.portId);
            firaDriverMutex.ReleaseMutex();
            statuSLabel.Text = "Robot " + robot.portId + " released!.";
            for (int i = 0; i < controlledRobots.Count; i++)
            {
                if (controlledRobots[i].getId() == robot.portId)
                {
                    controlledRobots.RemoveAt(i);
                    break;
                }
            }
        }

        private void viewPanel_Resize(object sender, System.EventArgs e)
        {
            CreateViewDrawer();
        }

        private void viewPanel_Paint(object sender, System.Windows.Forms.PaintEventArgs e)
        {
            if (viewPanelGraphics == null && viewBitmap == null)
                return;

            viewPanelGraphics.DrawImageUnscaled(viewBitmap, 0, 0);


            if (showPotentialField)
            {
                paintPotentialField();
            }

            foreach (FIRARobotState robot in firaDriver.firaRobots)
            {
                viewPanelGraphics.DrawEllipse((robot.owned ? theRobotPen : robotPen), Real2ViewX(robot.x) - robotDiameterView / 2, Real2ViewY(robot.y) - robotDiameterView / 2, robotDiameterView, robotDiameterView);
                viewPanelGraphics.DrawLine((robot.owned ? theRobotPen : robotPen), Real2ViewX(robot.x), Real2ViewY(robot.y), Real2ViewX(robot.x) + (float)(robotDiameterView * Math.Cos(robot.angle)), Real2ViewY(robot.y) + (float)(robotDiameterView * Math.Sin(robot.angle)));
            }

            viewPanelGraphics.DrawEllipse(ballPen, Real2ViewX(firaDriver.ballX) - ballDiameterView / 2, Real2ViewY(firaDriver.ballY) - ballDiameterView / 2, ballDiameterView, ballDiameterView);
            viewPanelGraphics.DrawEllipse(targetPen, Real2ViewX(targetPoint.x), Real2ViewY(targetPoint.y), 1, 1);

            //draw target path
            foreach (Point2D point in targets)
            {
                viewPanelGraphics.DrawEllipse(targetPathPen, Real2ViewX(point.x), Real2ViewY(point.y), 1, 1);
            }
            foreach (ControlledRobot robot in controlledRobots)
            {
                if (robot.getIsMoving() && targets.Count > 1 && robot.getCurrentTarget() < targets.Count - 1)
                {
                    viewPanelGraphics.DrawEllipse(currentTargetPathPen, Real2ViewX(targets[robot.getCurrentTarget()].x), Real2ViewY(targets[robot.getCurrentTarget()].y), 2, 2);
                }

            }
            //draw visited
            List<Point2D> visitedCopy = new List<Point2D>(visited);
            foreach (Point2D point in visitedCopy)
            {
                viewPanelGraphics.DrawEllipse(visitedPen, Real2ViewX(point.x), Real2ViewY(point.y), 1, 1);
            }

            foreach (Wall2D wall in pitch.getWalls())
                viewPanelGraphics.DrawLine(wallPen, Real2ViewX(wall.firstPoint.x), Real2ViewY(wall.firstPoint.y), Real2ViewX(wall.secondPoint.x), Real2ViewY(wall.secondPoint.y));


            lock (PotentialFieldDriver.PotencialDriver.additionalObstaclePoints)
            {
                foreach (Point2D point in PotentialFieldDriver.PotencialDriver.additionalObstaclePoints)
                    viewPanelGraphics.FillRectangle(additionalPointsBrush, Real2ViewX(point.x - 0.04), Real2ViewY(point.y - 0.04), 0.08f * viewScale, 0.08f * viewScale);
            }

            if (showBitmap)
            {
                paintBitmap();
            }


            // draw locks
            FIRARobotState selectedRobot = null;
            try
            {
                selectedRobot = robotsListBox.Items[robotsListBox.SelectedIndex] as FIRARobotState;
            }
            catch
            {
                return;
            }
            if (selectedRobot == null || PotencialDriver.angleLocks == null)
                return;

            //drawing angles locks and target angles

            //firaDriverMutex.WaitOne();
            //foreach (PotencialDriver.AngleLock angleLock in PotencialDriver.angleLocks)
            //{
            //    viewPanelGraphics.DrawLine(ballPen, Real2ViewX(selectedRobot.x), Real2ViewY(selectedRobot.y),
            //        Real2ViewX(selectedRobot.x) + (float)(Math.Cos(angleLock.lockVector.Angle + angleLock.halfAngle) * angleLock.lockVector.Length * viewScale),
            //        Real2ViewY(selectedRobot.y) + (float)(Math.Sin(angleLock.lockVector.Angle + angleLock.halfAngle) * angleLock.lockVector.Length * viewScale));
            //    viewPanelGraphics.DrawLine(ballPen, Real2ViewX(selectedRobot.x), Real2ViewY(selectedRobot.y),
            //        Real2ViewX(selectedRobot.x) + (float)(Math.Cos(angleLock.lockVector.Angle - angleLock.halfAngle) * angleLock.lockVector.Length * viewScale),
            //        Real2ViewY(selectedRobot.y) + (float)(Math.Sin(angleLock.lockVector.Angle - angleLock.halfAngle) * angleLock.lockVector.Length * viewScale));

            //    viewPanelGraphics.DrawEllipse(targetPen, Real2ViewX(PotencialDriver.checkPoint.x), Real2ViewY(PotencialDriver.checkPoint.y), 2, 2);
            //}
            //firaDriverMutex.ReleaseMutex();

        }

        private void paintPotentialField()
        {
            Bitmap bitmap = driver.getBitmap();
            if (bitmap == null)
            {
                return;
            }
            viewScale = (float)Math.Min(viewPanel.Width / (1.2f * pitchArea.Width),
                viewPanel.Height / (1.1f * pitchArea.Height));
            viewPanelGraphics.DrawImage(bitmap, Real2ViewX(pitchArea.X), Real2ViewY(pitchArea.Y), pitchArea.Width * viewScale, pitchArea.Height * viewScale);
        }

        private void paintBitmap()
        {
            if (pitch == null)
            {
                return;
            }
            Cell[][] bitmap = pitch.getObstaclesBitmap(noOfColumnsInGrid, noOfRowsInGrid);
            for (int i = 0; i < bitmap.Length; i++)
            {
                for (int j = 0; j < bitmap[i].Length; j++)
                {
                    if (bitmap[i][j].isObstacle())
                    {
                        viewPanelGraphics.FillRectangle(obstaclesBrush, Real2ViewX(bitmap[i][j].getUpperLeftPoint().x), Real2ViewY(bitmap[i][j].getUpperLeftPoint().y), (float)(bitmap[i][j].getWidth() * viewScale), (float)(bitmap[i][j].getHeight() * viewScale));
                    }
                    else if (bitmap[i][j].Reachable)
                    {
                        viewPanelGraphics.FillRectangle(nonReachableBrush, Real2ViewX(bitmap[i][j].getUpperLeftPoint().x), Real2ViewY(bitmap[i][j].getUpperLeftPoint().y), (float)(bitmap[i][j].getWidth() * viewScale), (float)(bitmap[i][j].getHeight() * viewScale));
                    }
                }
            }
        }

        private void interfaceRefreshTimer_Tick(object sender, System.EventArgs e)
        {
            viewPanel_Paint(this, null);
        }


        private void viewPanel_MouseUp(object sender, System.Windows.Forms.MouseEventArgs e)
        {
            if (addWallCheckBox.Checked || addPointCheckBox.Checked)
            {
                if (addWallCheckBox.Checked)
                {
                    addWallCheckBox.Checked = false;
                    lock (PotencialDriver.walls)
                    {
                        PotencialDriver.walls.Add(new Wall2D(targetPoint, new Point2D(View2RealX(e.X), View2RealY(e.Y))));
                    }
                }

                if (addPointCheckBox.Checked)
                {
                    addPointCheckBox.Checked = false;
                    lock (PotencialDriver.additionalObstaclePoints)
                    {
                        PotencialDriver.additionalObstaclePoints.Add(new Point2D(View2RealX(e.X), View2RealY(e.Y)));
                    }
                }
            }
            else
            {
                targetPoint = new Point2D(View2RealX(e.X), View2RealY(e.Y));
                targetLocationLabel.Text = "target location: \n" + targetPoint.ToLongString("f2");
                if (targets.Count == 0)
                {
                    lock (targets)
                    {
                        targets.Add(targetPoint);
                    }
                    targetLocationLabel.Text = "added target: \n" + targetPoint.ToLongString("f2");
                }
                else
                {
                    bool collisionDetected = false;
                    foreach (Wall2D wall in PotencialDriver.walls)
                    {
                        if (CollisionChecker.checkCollicionSection(targets[targets.Count - 1], targetPoint, wall.firstPoint, wall.secondPoint))
                        {
                            collisionDetected = true;
                            break;
                        }
                    }
                    if (!collisionDetected)
                    {
                        lock (targets)
                        {
                            targets.Add(targetPoint);
                        }
                        targetLocationLabel.Text = "added target: \n" + targetPoint.ToLongString("f2");
                    }
                    else
                    {
                        targetLocationLabel.Text = "path colliding: \n" + targetPoint.ToLongString("f2");
                    }
                }
            }
        }

        private void moveRobotButton_Click(object sender, System.EventArgs e)
        {
            foreach (ControlledRobot robot in controlledRobots)
            {
                if (targets.Count > 0)
                {
                    if (targets.Count <= robot.getCurrentTarget())
                    {
                        robot.setCurrentTarget(0);
                    }
                    robot.setIsMoving(true);
                    //currentTarget = 0;
                    //movingRobotId = robotsListBox.SelectedIndex;
                }
                else
                {
                    Console.WriteLine("No tragets selected!");
                }
            }
        }

        private void stopRobotButton_Click(object sender, EventArgs e)
        {
            foreach (ControlledRobot robot in controlledRobots)
            {
                robot.setIsMoving(false);
                firaDriver.firaRobots[robot.getId()].leftSpeed = 0;
                firaDriver.firaRobots[robot.getId()].rightSpeed = 0;
            }
        }

        private void rightStatusLabel_DoubleClick(object sender, System.EventArgs e)
        {
            oldTime = -1;
        }

        private void clearPointsButton_Click(object sender, System.EventArgs e)
        {
            lock (PotencialDriver.additionalObstaclePoints)
            {
                PotencialDriver.additionalObstaclePoints.Clear();
            }
        }

        private void clearWallsButton_Click(object sender, System.EventArgs e)
        {
            lock (PotencialDriver.walls)
            {
                PotencialDriver.walls.Clear();
            }
        }

        //add sample walls 1
        private void pitchBoundsButton_Click(object sender, System.EventArgs e)
        {
            lock (PotencialDriver.walls)
            {
                PotencialDriver.walls.Clear();
                double middleH = (PotentialFieldDriver.PotencialDriver.pitchArea.Left + PotentialFieldDriver.PotencialDriver.pitchArea.Right) / 2;
                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left, PotentialFieldDriver.PotencialDriver.pitchArea.Top),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right, PotentialFieldDriver.PotencialDriver.pitchArea.Top));
                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right, PotentialFieldDriver.PotencialDriver.pitchArea.Top),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom));
                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom));
                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left, PotentialFieldDriver.PotencialDriver.pitchArea.Top));

                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left + 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Top + 0.3),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right - 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Top + 0.3));
                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right - 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Top + 0.3),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right - 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3));


                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Right - 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3),
                    new Point2D(middleH + 0.5, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3));
                addWallToPotentialDriver(
                    new Point2D(middleH - 0.5, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left + 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3));

                addWallToPotentialDriver(
                    new Point2D(middleH, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom),
                    new Point2D(middleH, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.5));



                addWallToPotentialDriver(
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left + 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Bottom - 0.3),
                    new Point2D(PotentialFieldDriver.PotencialDriver.pitchArea.Left + 0.3, PotentialFieldDriver.PotencialDriver.pitchArea.Top + 0.3));
            }

        }


        //add sample walls 2
        private void pitchBoundsButton2_Click(object sender, EventArgs e)
        {
            PotencialDriver.walls.Clear();
            double middleH = (PotentialFieldDriver.PotencialDriver.pitchArea.Left + PotentialFieldDriver.PotencialDriver.pitchArea.Right) / 2;
            double middleHL = (PotentialFieldDriver.PotencialDriver.pitchArea.Left + middleH) / 2;
            double middleHR = (middleH + PotentialFieldDriver.PotencialDriver.pitchArea.Right) / 2;

            double middleV = (PotentialFieldDriver.PotencialDriver.pitchArea.Top + PotentialFieldDriver.PotencialDriver.pitchArea.Bottom) / 2;
            double middleVT = (PotentialFieldDriver.PotencialDriver.pitchArea.Top + middleV) / 2;
            double middleVB = (middleV + PotentialFieldDriver.PotencialDriver.pitchArea.Bottom) / 2;

            addWallToPotentialDriver(
                    new Point2D(middleHR, middleVT),
                    new Point2D(middleHR, middleVB));

            addWallToPotentialDriver(
                    new Point2D(middleHL, middleVT),
                    new Point2D(middleHL, middleVB));
        }


        //add wall to potentialDriver.walls and if too long split and add multiple walls
        private void addWallToPotentialDriver(Point2D firstPoint, Point2D secondPoint)
        {
            double wallLength = firstPoint.GetDistance(secondPoint);
            if (wallLength > 0.3)
            {
                addWallToPotentialDriver(firstPoint, new Point2D((firstPoint.x + secondPoint.x) / 2, (firstPoint.y + secondPoint.y) / 2));
                addWallToPotentialDriver(new Point2D((firstPoint.x + secondPoint.x) / 2, (firstPoint.y + secondPoint.y) / 2), secondPoint);
            }
            else
            {
                PotencialDriver.walls.Add(new Wall2D(firstPoint, secondPoint));
            }
        }

        private void PotencialDriverForm_Load(object sender, System.EventArgs e)
        {

        }

        private void restPathButton_Click(object sender, EventArgs e)
        {
            targets.Clear();
        }

        //optimize path
        private void optimizePathButton_Click(object sender, EventArgs e)
        {
            bool collisionDetected = false;
            List<Point2D> tempTargets = new List<Point2D>(targets);
            tempTargets.Add(targets[0]);
            collisionDetected = !PathOptimizer.isPossible(tempTargets, PotencialDriver.walls);
            if (collisionDetected)
            {
                targetLocationLabel.Text = "path colliding!!";
                return;

            }
            targets.Add(targetPoint);
            targetLocationLabel.Text = "Optimizing path...";
            targets = PathOptimizer.optimize(tempTargets, PotencialDriver.walls, 0.2, 0.06);
            targetLocationLabel.Text = "Optimization done\nGenerating fields...";
            driver = new PotentialFieldBasedDriver(pitch, targets, Convert.ToInt32(wallRadix.Value));
            Console.WriteLine("Fields done");
        }

        private void ShowBitmap_Click(object sender, EventArgs e)
        {
            this.showBitmap = !this.showBitmap;
            Console.WriteLine("Show bitmap: " + showBitmap);
            viewPanel_Paint(null, null);
        }

        private void showPotentialField_Click(object sender, EventArgs e)
        {
            this.showPotentialField = !this.showPotentialField;
            viewPanel_Paint(null, null);
        }

        private void numericUpDown1_ValueChanged(object sender, EventArgs e)
        {
            PotentialFieldBasedDriver pfdriver = driver as PotentialFieldBasedDriver;
            if (pfdriver != null)
            {
                pfdriver.setWallRadix(Convert.ToInt32(wallRadix.Value));
                viewPanel_Paint(null, null);
            }
        }
    }
}
